package novemberBot.controller;

import lejos.nxt.Button;
import lejos.nxt.ButtonListener;
import lejos.nxt.MotorPort;
import lejos.nxt.NXT;
import lejos.nxt.comm.RS485;
import novemberBot.Arm;
import novemberBot.communication.*;
import novemberBot.sensor.LSPoller;
/**Main class for slave brick
 * @author Antoine Lennartz*/
public class SlaveMain {
	/**Connection Manager*/
	public static ConnectionManager cm;
	/**Link to arm object*/
	public static Arm theArm;
	/**link to light sensor poller*/
	public static LSPoller lsp;
	
	/**main class for slave brick*/
	public static void main(String args[]){	 
		initialize(true);
		//testClaw();
		
	}
	/**Establishes connection an initialises the class objects
	 * @param connect -  connects or not to other brick*/
	public static void initialize(boolean connect){
		theArm = new Arm(MotorPort.C, 1, MotorPort.B, 1);
		if(connect){
			System.out.println("Waiting ...");
			cm = new ConnectionManager(RS485.waitForConnection(0, 1), true);
			System.out.println("Connected!");
			cm.addResponder(theArm);
			lsp = new LSPoller(cm);		
		}else System.out.println("no connection");
		Button.ESCAPE.addButtonListener(new ButtonListener() {
			
			@Override
			public void buttonPressed (Button b){
				theArm.liftClawTo(0);
			}
			@Override
			public void buttonReleased (Button b){
				try{Thread.sleep((long) (1100/Arm.LIFT_SPEED*theArm.clawHeight*Arm.HEIGHT_TO_DEG));}catch(Exception e){}
				NXT.exit(0);
			}
			
		});
	}
	
	public static void testClaw(){
		theArm.liftClawTo(6);
		try{Thread.sleep((long) (1100/Arm.LIFT_SPEED*6*Arm.HEIGHT_TO_DEG));}catch(Exception e){}
		while(true){
			Button.waitForPress();
			theArm.liftClawTo(5);
			try{Thread.sleep((long) (1100/Arm.LIFT_SPEED*Arm.HEIGHT_TO_DEG));}catch(Exception e){}
			theArm.openClawTo(35);
			try{Thread.sleep((long) (1100/Arm.OPEN_SPEED*35));}catch(Exception e){}
			theArm.liftClawTo(0);
			try{Thread.sleep((long) (1100/Arm.LIFT_SPEED*5*Arm.HEIGHT_TO_DEG));}catch(Exception e){}
			theArm.openClawTo(-30);
			try{Thread.sleep((long) (1100/Arm.OPEN_SPEED*40));}catch(Exception e){}
			theArm.liftClawTo(6);
			try{Thread.sleep((long) (1100/Arm.LIFT_SPEED*6*Arm.HEIGHT_TO_DEG));}catch(Exception e){}
			
		}
	}
}
